/*************************************************
 @copyright��IFR(2021)
 @filename: SMProperty.c
 
 @description: void function related to speed moto ;store mcu command,send frame
 @Currant: V1.0
 @author: zdh
 @finishdate: 
*************************************************/


/***********User Define Begin ***************/


/***********User Define End ***************/



/*               User Include Begin             */


#include "SMProperty.h"
#include "can.h"


/*               User Include End             */


/*********  User Extern Begin ************/
extern RPDO_CANFrame_t 				RPDO_CANFrame;
extern struct rt_messagequeue 		Queen_StandardCANFrame_SpeedDataToMoto;							//打包好的速度电机数据到发送给电机任务的消息队列
extern struct rt_messagequeue  		Queen_SpeedStandardMotoData;									//分发标准速度数据给速度存储任务的消息队列
extern struct rt_mutex 				Mutex_SpeedTarStructLock; 
extern uint8_t 						NewSpeedData_flag;  //是否可以发送打包好的speed数据入队的标志位
/*********  User Extern End **************/



/********* User Golbal Variable Begin  ************/

/***************************           Node         ************************************/
CANopen_Node_t 			SpeedMotoNode[MAX_SPEEDMOTONUM];   //16�������Node�ڵ�

SDO_t 					SpeedNodeSDO[MAX_SPEEDNODE_SDO];

RPDO_t					SpeedNode_RPDO[MAX_SPEEDMOTONUM][MAX_SPEEDNODE_RPDO_MUM]; //ÿ��������ĸ�RPDO

TPDO_t 					SpeedNode_TPDO[MAX_SPEEDMOTONUM][MAX_SPEEDNODE_TPDO_NUM]; // ÿ��������ĸ�TPDO

RPDO_MapObject_t 		Set_RPDO_MapObject[MAX_SPEEDMOTONUM];    //ÿ���������RPDOӳ�����
TPDO_MapObject_t		Set_TPDO_MapObject[MAX_SPEEDMOTONUM];	//ÿ���������TPDOӳ�����

uint8_t 				Set_RPDO_TransType[MAX_SPEEDMOTONUM][MAX_SPEEDNODE_RPDO_MUM]; //ÿ�������ÿ��RPDO����ͨ�Ų���
uint8_t 				Set_RPDO_MapNum[MAX_SPEEDMOTONUM][MAX_SPEEDNODE_RPDO_MUM];	//ÿ�������ÿ��RPDO����ӳ���������

uint8_t 				Set_TPDO_TransType[MAX_SPEEDMOTONUM][MAX_SPEEDNODE_TPDO_NUM];//ÿ�������ÿ��TPDO����ͨ�Ų���
uint8_t 				Set_TPDO_MapNum[MAX_SPEEDMOTONUM][MAX_SPEEDNODE_TPDO_NUM];//ÿ�������ÿ��TPDO����ӳ���������
/***************************           Node         ************************************/


/************************          Moto           **************************************/
Whole_SpeedMoto_t  			SpeedMotoStruct[MAX_SPEEDMOTONUM];  	//	速度电机总结构体  

SpeedMotoTarData_t 			SpeedMotoTarData[MAX_SPEEDMOTONUM];  	//速度电机目标数据结构体

SpeedMotoFeedBackData_t 	SpeedMotoFeedBackData[MAX_SPEEDMOTONUM]; //速度电机反馈数据结构体
/************************          Moto           **************************************/


/********** User Golbal Variable End *************/



/*****************************************
 @function:�Ե����NodeID���г�ʼ����ֵ
 @param:NULL
author:GTRLTABS
********************************************/
void SpeedMotoNodeIDInstantiate(void)
{
	SpeedMotoNode[SMOTO_1].nodeID = 1;
	SpeedMotoNode[SMOTO_2].nodeID = 2;
	SpeedMotoNode[SMOTO_3].nodeID = 3;
	SpeedMotoNode[SMOTO_4].nodeID = 4;
	SpeedMotoNode[SMOTO_5].nodeID = 5;
	SpeedMotoNode[SMOTO_6].nodeID = 6;
	SpeedMotoNode[SMOTO_7].nodeID = 7;
	SpeedMotoNode[SMOTO_8].nodeID = 8;
	SpeedMotoNode[SMOTO_9].nodeID = 9;
	SpeedMotoNode[SMOTO_10].nodeID = 10;
	SpeedMotoNode[SMOTO_11].nodeID = 11;
	SpeedMotoNode[SMOTO_12].nodeID = 12;
	SpeedMotoNode[SMOTO_13].nodeID = 13;
	SpeedMotoNode[SMOTO_14].nodeID = 14;
	SpeedMotoNode[SMOTO_15].nodeID = 15;
	SpeedMotoNode[SMOTO_16].nodeID = 16;
	
	for(uint8_t motonum = 0; motonum <MAX_SPEEDMOTONUM; motonum++)
	{
		SpeedMotoNode[motonum].speed_Or_Position_mode = SPEED_MODE;
	}
}

/*****************************************
 @function:���ٶȵ����SDO����ָ��ʵ����
 @param:NULL
author:GTRLTABS
********************************************/
void SpeedMotoSDOInstantiate(void)
{
	SpeedMotoNode[SMOTO_1].nodeSDO  =  &SpeedNodeSDO[S_NODESDO_1];
	SpeedMotoNode[SMOTO_2].nodeSDO  =  &SpeedNodeSDO[S_NODESDO_2];
	SpeedMotoNode[SMOTO_3].nodeSDO  =  &SpeedNodeSDO[S_NODESDO_3];
	SpeedMotoNode[SMOTO_4].nodeSDO  =  &SpeedNodeSDO[S_NODESDO_4];
	SpeedMotoNode[SMOTO_5].nodeSDO  =  &SpeedNodeSDO[S_NODESDO_5];
	SpeedMotoNode[SMOTO_6].nodeSDO  =  &SpeedNodeSDO[S_NODESDO_6];
	SpeedMotoNode[SMOTO_7].nodeSDO  =  &SpeedNodeSDO[S_NODESDO_7];
	SpeedMotoNode[SMOTO_8].nodeSDO  =  &SpeedNodeSDO[S_NODESDO_8];
	SpeedMotoNode[SMOTO_9].nodeSDO  =  &SpeedNodeSDO[S_NODESDO_9];
	SpeedMotoNode[SMOTO_10].nodeSDO =  &SpeedNodeSDO[S_NODESDO_10];
	SpeedMotoNode[SMOTO_11].nodeSDO =  &SpeedNodeSDO[S_NODESDO_11];
	SpeedMotoNode[SMOTO_12].nodeSDO =  &SpeedNodeSDO[S_NODESDO_12];
	SpeedMotoNode[SMOTO_13].nodeSDO =  &SpeedNodeSDO[S_NODESDO_13];
	SpeedMotoNode[SMOTO_14].nodeSDO =  &SpeedNodeSDO[S_NODESDO_14];
	SpeedMotoNode[SMOTO_15].nodeSDO =  &SpeedNodeSDO[S_NODESDO_15];
	SpeedMotoNode[SMOTO_16].nodeSDO =  &SpeedNodeSDO[S_NODESDO_16];
	
	for(uint8_t motonum = 0; motonum <MAX_SPEEDMOTONUM; motonum++) //��ʼ״̬��SDO��ΪNormal״̬
	{
		SpeedMotoNode[motonum].nodeSDO->SD0_Status = SDO_NORMAL;
		SpeedMotoNode[motonum].nodeSDO->SDO_outTime_Count = SDO_OUTTIME_SET;		//������SDO�ĵȴ���ʱʱ�����ó�ʼֵ
		SpeedMotoNode[motonum].nodeSDO->SDO_errorTime_Count = SDO_ERRORTIME_SET;	//������SDO����Ӧ����״̬�ĳ�ʱʱ�����ó�ʼֵ
	}
	
	for(uint8_t motonum = 0; motonum <MAX_SPEEDMOTONUM; motonum++) 
	{
		SpeedMotoNode[motonum].nodeSDO->SDO_waitResponse_watchDog = SDO_WaitResponse_WatchDog;
		SpeedMotoNode[motonum].nodeSDO->SDO_errorState_watchDog = SDO_ErrorState_WatchDog;
	}
}

/*****************************************
 @function:���ٶȵ��������ʼ״̬���г�ʼ����ֵ
 @param:NULL
author:GTRLTABS
********************************************/
void SpeedMotoNodeBootupStateInit(void)
{
	for(uint8_t motonum = 0; motonum<MAX_SPEEDMOTONUM; motonum++)
	{
		SpeedMotoNode[motonum].NodeBootupStateMachine = BOOTUP_NMT_SET;
	}

}

/*****************************************
 @function:���ٶȵ������ʱ����г�ʼ����ֵ
 @param:NULL
author:GTRLTABS
********************************************/
void SpeedMotoNodeNMTHeartbeatConfig(void)
{
	for(uint8_t motonum = 0; motonum<MAX_SPEEDMOTONUM; motonum++)
	{
		SpeedMotoNode[motonum].NMTNodeHeartBeatTimeConfig.heartBeatFeedBack_Cycle	= SM_HEARTBEAT_CYCLE;  //������������Ϊ50
		SpeedMotoNode[motonum].NMTNodeHeartBeatTimeConfig.heartBeatFeedBack_OutTime = SM_HEARTBEAT_OUTTIME;//������������ʱʱ������Ϊ3*50��δ�յ���Ϊ���ߡ�
	}
}

/*****************************************
 @function:���ٶȵ����RPDO����ָ��ʵ����
 @param:NULL
author:GTRLTABS
********************************************/
void SpeedMotoRPDOInstantiate(void)
{
	
	for (uint8_t motonum = 0; motonum<MAX_SPEEDMOTONUM; motonum++)
	{
		for (uint8_t RPDOnum = 0; RPDOnum < MAX_SPEEDNODE_RPDO_MUM; RPDOnum++)
		{
			SpeedMotoNode[motonum].nodeRPDO[RPDOnum] = &SpeedNode_RPDO[motonum][RPDOnum];
			
			SpeedMotoNode[motonum].nodeRPDO[RPDOnum]->RPDO_num = RPDOnum; //��RPDO�Ž������Ը�ֵ
		}
	}
}

/*****************************************
 @function:���ٶȵ����TPDO����ָ��ʵ����
 @param:NULL
author:GTRLTABS
********************************************/
void SpeedMotoTPDOInstantiate(void)
{
	for (uint8_t motonum = 0; motonum<MAX_SPEEDMOTONUM; motonum++)
	{
		for (uint8_t TPDOnum = 0; TPDOnum < MAX_SPEEDNODE_TPDO_NUM; TPDOnum++)
		{
			SpeedMotoNode[motonum].nodeTPDO[TPDOnum]  =  &SpeedNode_TPDO[motonum][TPDOnum];
			SpeedMotoNode[motonum].nodeTPDO[TPDOnum]->TPDO_num = TPDOnum;
		}
	}	
}

void JMC_SpeedMotoRPDOConfig(void)
{
	SpeedMotoNode[SMOTO_1].nodeRPDO[RPDO1]->RPDO_isuse = USE_THIS_RPDO;
	SpeedMotoNode[SMOTO_2].nodeRPDO[RPDO1]->RPDO_isuse = USE_THIS_RPDO;
	SpeedMotoNode[SMOTO_3].nodeRPDO[RPDO1]->RPDO_isuse = USE_THIS_RPDO;
	SpeedMotoNode[SMOTO_4].nodeRPDO[RPDO1]->RPDO_isuse = USE_THIS_RPDO;
	
	Set_RPDO_TransType[SMOTO_1][RPDO1] = 0;
	Set_RPDO_TransType[SMOTO_2][RPDO1] = 0;
	Set_RPDO_TransType[SMOTO_3][RPDO1] = 0;
	Set_RPDO_TransType[SMOTO_4][RPDO1] = 0;
	
	Set_RPDO_MapNum[SMOTO_1][RPDO1]  = 3;
	Set_RPDO_MapNum[SMOTO_2][RPDO1]  = 3;
	Set_RPDO_MapNum[SMOTO_3][RPDO1]  = 3;
	Set_RPDO_MapNum[SMOTO_4][RPDO1]  = 3;
	
	Set_RPDO_MapObject[SMOTO_1].RPDO1_MapObject[RPDO_MAP_OBJECT_1] = 0x60810020;		//目标速度
	Set_RPDO_MapObject[SMOTO_1].RPDO1_MapObject[RPDO_MAP_OBJECT_2] = 0x60830010;		//加速度
	Set_RPDO_MapObject[SMOTO_1].RPDO1_MapObject[RPDO_MAP_OBJECT_3] = 0x60840010;		//减速度
	SpeedMotoNode[SMOTO_1].nodeRPDO[RPDO1]->RPDO_datalen 		   = 8;
	
	Set_RPDO_MapObject[SMOTO_2].RPDO1_MapObject[RPDO_MAP_OBJECT_1] = 0x60810020;
	Set_RPDO_MapObject[SMOTO_2].RPDO1_MapObject[RPDO_MAP_OBJECT_2] = 0x60830010;
	Set_RPDO_MapObject[SMOTO_2].RPDO1_MapObject[RPDO_MAP_OBJECT_3] = 0x60840010;
	SpeedMotoNode[SMOTO_2].nodeRPDO[RPDO1]->RPDO_datalen 		   = 8;
	
	Set_RPDO_MapObject[SMOTO_3].RPDO1_MapObject[RPDO_MAP_OBJECT_1] = 0x60810020;
	Set_RPDO_MapObject[SMOTO_3].RPDO1_MapObject[RPDO_MAP_OBJECT_2] = 0x60830010;
	Set_RPDO_MapObject[SMOTO_3].RPDO1_MapObject[RPDO_MAP_OBJECT_3] = 0x60840010;
	SpeedMotoNode[SMOTO_3].nodeRPDO[RPDO1]->RPDO_datalen 		   = 8;
	
	Set_RPDO_MapObject[SMOTO_4].RPDO1_MapObject[RPDO_MAP_OBJECT_1] = 0x60810020;
	Set_RPDO_MapObject[SMOTO_4].RPDO1_MapObject[RPDO_MAP_OBJECT_2] = 0x60830010;
	Set_RPDO_MapObject[SMOTO_4].RPDO1_MapObject[RPDO_MAP_OBJECT_3] = 0x60840010;
	SpeedMotoNode[SMOTO_4].nodeRPDO[RPDO1]->RPDO_datalen 		   = 8;
	
	SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->tar_Speed = 0;
	SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->tar_accel = 1000;
	SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->tar_decel = 1000;
	
	SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->tar_Speed = 0;
	SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->tar_accel = 1000;
	SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->tar_decel = 1000;
	
	SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->tar_Speed = 0;
	SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->tar_accel = 1000;
	SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->tar_decel = 1000;
	
	SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->tar_Speed = 0;
	SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->tar_accel = 1000;
	SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->tar_decel = 1000;
}


/*****************************************
 @function:杰美康电机的RPDO拼帧函数，根据对应的RPDO顺序进行拼帧
 @param:*SpeedMotoNode ：指示哪个节点 
 @param: *moto:  指示是哪个节点对应的电机
 @param: *RPDO_canframe  :RPDOCAN帧发送结构体 
author:GTRLTABS
********************************************/
uint8_t NewSpeedData_flag = 0;  //在StoreSpeedData2Struct 任务中更新此标志位
rt_err_t 	monitor_mq;
int32_t test_jmc_tar_speed;
void JMC_PackSpeedRPDOConrtolFrame(CANopen_Node_t *SpeedMotoNode, Whole_SpeedMoto_t *moto)
{
	
	int32_t jmc_tar_speed;
	uint16_t jmc_tar_acc;
	uint16_t jmc_tar_deacc;
	
	RPDO_CANFrame_t  RPDO_canframe;
	
	RPDO_canframe.RPDO_RTR = CAN_RTR_DATA;  
	
	for(uint8_t RPDO_num = 0; RPDO_num <MAX_SPEEDNODE_RPDO_MUM;RPDO_num++)
	{
		if(SpeedMotoNode->nodeRPDO[RPDO_num]->RPDO_isuse == USE_THIS_RPDO)
		{
			switch (SpeedMotoNode->nodeRPDO[RPDO_num]->RPDO_num)
			{
				case RPDO1:
				{
					RPDO_canframe.RPDO_ID = 0x200 + SpeedMotoNode->nodeID;
					
					
//					if (RPDO_canframe.RPDO_ID == 0x202 || RPDO_canframe.RPDO_ID == 0x204)
//					{
//						jmc_tar_speed = (int32_t)( -(moto->SpeedMotoTarData->tar_Speed * 10.f/60.f) ); 
//						
//					}
					jmc_tar_speed = (int32_t)(moto->SpeedMotoTarData->tar_Speed * 10.f/60.f); 
					
					/* 3000rpm的限幅 */
					if (jmc_tar_speed >= 500 )
					{
						jmc_tar_speed = 500;
					}	
					if (jmc_tar_speed <= -500)
					{
						jmc_tar_speed = -500;
					}
					/* 3000rpm的限幅 */
					
					
					jmc_tar_acc = (uint16_t)moto->SpeedMotoTarData->tar_accel;
					jmc_tar_deacc = (uint16_t)moto->SpeedMotoTarData->tar_decel;
					
					
					memcpy(RPDO_canframe.RPDO_Data , &(jmc_tar_speed), BYTE_4);
					
					
					memcpy( &(RPDO_canframe.RPDO_Data[4]) , &(jmc_tar_acc), BYTE_2);
					
					memcpy( &(RPDO_canframe.RPDO_Data[6]) , &(jmc_tar_deacc), BYTE_2);
					
					RPDO_canframe.RPDO_DATA_DLC = SpeedMotoNode->nodeRPDO[RPDO_num]->RPDO_datalen;
					break;
				}
				case RPDO2:
				{
					RPDO_canframe.RPDO_ID = 0x300 + SpeedMotoNode->nodeID;  // 速度模式目前没用别的RPDO
					break;
				}
				case RPDO3:
				{
					RPDO_canframe.RPDO_ID = 0x400 + SpeedMotoNode->nodeID; // 速度模式目前没用别的RPDO
					break;
				}
				case RPDO4:
				{
					RPDO_canframe.RPDO_ID = 0x500 + SpeedMotoNode->nodeID; // 速度模式目前没用别的RPDO
					break;
				}
				default :
				{
					break;
				}
			
			}//switch (SpeedMotoNode->nodeRPDO[RPDO_num]->RPDO_num)
			
		}//if(SpeedMotoNode->nodeRPDO[RPDO_num]->RPDO_isuse == USE_THIS_RPDO)
		
		
//		if (NewSpeedData_flag == 1)  //监视加负号之后入队的数据是多少
//		{
			monitor_mq = rt_mq_send_wait(&Queen_StandardCANFrame_SpeedDataToMoto,&RPDO_canframe, sizeof(RPDO_CANFrame_t),10);//将打包好的数据放入队列
//			NewSpeedData_flag = 0;
//		}
		
	}//for(uint8_t RPDO_num = 0; RPDO_num <MAX_SPEEDNODE_RPDO_MUM;RPDO_num++)
	
}

RPDO_CANFrame_t testpdo;
uint32_t test_lms_tar_acc;
uint32_t test_lms_tar_deacc;
int32_t test_lms_tar_speed;


void LMS_PackSpeedRPDOConrtolFrame(CANopen_Node_t *SpeedMotoNode, Whole_SpeedMoto_t *moto)
{
	
	
	RPDO_CANFrame_t  RPDO_canframe;
	
	RPDO_canframe.RPDO_RTR = CAN_RTR_DATA;  
	int32_t lms_tar_speed;
	uint32_t lms_tar_acc;
	uint32_t lms_tar_deacc;
	
	uint32_t lms_tar_a_acc;
	uint32_t lms_tar_d_deacc;
	
	for(uint8_t RPDO_num = 0; RPDO_num <MAX_SPEEDNODE_RPDO_MUM;RPDO_num++)
	{
		if(SpeedMotoNode->nodeRPDO[RPDO_num]->RPDO_isuse == USE_THIS_RPDO)
		{
			switch (SpeedMotoNode->nodeRPDO[RPDO_num]->RPDO_num)
			{
				case RPDO1:
				{
					RPDO_canframe.RPDO_ID = 0x200 + SpeedMotoNode->nodeID;
					

					lms_tar_speed = (int32_t)(moto->SpeedMotoTarData->tar_Speed * 20000/60);//将速度单位转化为rpm


					
					/*速度限幅*/
					if(lms_tar_speed>500000)
					{
						lms_tar_speed = 500000;
					}
					else if(lms_tar_speed < -500000)
					{
						lms_tar_speed = -500000;
					}
					/*速度限幅*/
					
					test_lms_tar_speed = lms_tar_speed;
					memcpy(RPDO_canframe.RPDO_Data , &(lms_tar_speed), BYTE_4);
					
				
					RPDO_canframe.RPDO_DATA_DLC = SpeedMotoNode->nodeRPDO[RPDO_num]->RPDO_datalen;
					
					break;
				}
				case RPDO2:
				{
					RPDO_canframe.RPDO_ID = 0x300 + SpeedMotoNode->nodeID;  
					
					lms_tar_acc = (uint32_t)(moto->SpeedMotoTarData->tar_accel * 60000) ;
					lms_tar_deacc = (uint32_t)(moto->SpeedMotoTarData->tar_decel * 60000);
					test_lms_tar_acc = lms_tar_acc;
					test_lms_tar_deacc = lms_tar_deacc;
					memcpy( &(RPDO_canframe.RPDO_Data), &(lms_tar_acc), BYTE_4);
					memcpy( &(RPDO_canframe.RPDO_Data[4]), &(lms_tar_deacc), BYTE_4);
					
					testpdo = RPDO_canframe;
					RPDO_canframe.RPDO_DATA_DLC = SpeedMotoNode->nodeRPDO[RPDO_num]->RPDO_datalen;
					
					
					break;
				}
				case RPDO3:
				{
					RPDO_canframe.RPDO_ID = 0x400 + SpeedMotoNode->nodeID; // 速度模式目前没用别的RPDO
					
					lms_tar_a_acc =   (uint32_t)(moto->SpeedMotoTarData->tar_a_accel * 100);
					lms_tar_d_deacc = (uint32_t)(moto->SpeedMotoTarData->tar_d_deccel * 100);
					
					memcpy( &(RPDO_canframe.RPDO_Data), &(lms_tar_a_acc), BYTE_4);
					memcpy( &(RPDO_canframe.RPDO_Data[4]), &(lms_tar_d_deacc), BYTE_4);
					
					RPDO_canframe.RPDO_DATA_DLC = SpeedMotoNode->nodeRPDO[RPDO_num]->RPDO_datalen;
					break;
				}
				case RPDO4:
				{
					RPDO_canframe.RPDO_ID = 0x500 + SpeedMotoNode->nodeID; // 速度模式目前没用别的RPDO
					break;
				}
				default :
				{
					break;
				}
			
			}//switch (SpeedMotoNode->nodeRPDO[RPDO_num]->RPDO_num)
			
		}//if(SpeedMotoNode->nodeRPDO[RPDO_num]->RPDO_isuse == USE_THIS_RPDO)
		
		
		rt_mq_send_wait(&Queen_StandardCANFrame_SpeedDataToMoto,&RPDO_canframe, sizeof(RPDO_CANFrame_t),1000);//将打包好的数据放入队列
	}//for(uint8_t RPDO_num = 0; RPDO_num <MAX_SPEEDNODE_RPDO_MUM;RPDO_num++)
	
}


void JMC_SpeedMotoTPDOConfig(void)
{
	SpeedMotoNode[SMOTO_1].nodeTPDO[TPDO1]->TPDO_isuse = USE_THIS_TPDO;
	SpeedMotoNode[SMOTO_2].nodeTPDO[TPDO1]->TPDO_isuse = USE_THIS_TPDO;
	SpeedMotoNode[SMOTO_3].nodeTPDO[TPDO1]->TPDO_isuse = USE_THIS_TPDO;
	SpeedMotoNode[SMOTO_4].nodeTPDO[TPDO1]->TPDO_isuse = USE_THIS_TPDO;
	
	SpeedMotoNode[SMOTO_1].nodeTPDO[TPDO2]->TPDO_isuse = USE_THIS_TPDO;
	SpeedMotoNode[SMOTO_2].nodeTPDO[TPDO2]->TPDO_isuse = USE_THIS_TPDO;
	SpeedMotoNode[SMOTO_3].nodeTPDO[TPDO2]->TPDO_isuse = USE_THIS_TPDO;
	SpeedMotoNode[SMOTO_4].nodeTPDO[TPDO2]->TPDO_isuse = USE_THIS_TPDO;
	
	Set_TPDO_TransType[SMOTO_1][TPDO1] = 1;
	Set_TPDO_TransType[SMOTO_2][TPDO1] = 1;
	Set_TPDO_TransType[SMOTO_3][TPDO1] = 1;
	Set_TPDO_TransType[SMOTO_4][TPDO1] = 1;
	
	Set_TPDO_TransType[SMOTO_1][TPDO2] = 1;
	Set_TPDO_TransType[SMOTO_2][TPDO2] = 1;
	Set_TPDO_TransType[SMOTO_3][TPDO2] = 1;
	Set_TPDO_TransType[SMOTO_4][TPDO2] = 1;
	
	Set_TPDO_MapNum[SMOTO_1][TPDO1]  = 2;
	Set_TPDO_MapNum[SMOTO_2][TPDO1]  = 2;
	Set_TPDO_MapNum[SMOTO_3][TPDO1]  = 2;
	Set_TPDO_MapNum[SMOTO_4][TPDO1]  = 2;
	
	Set_TPDO_MapNum[SMOTO_1][TPDO2]  = 2;
	Set_TPDO_MapNum[SMOTO_2][TPDO2]  = 2;
	Set_TPDO_MapNum[SMOTO_3][TPDO2]  = 2;
	Set_TPDO_MapNum[SMOTO_4][TPDO2]  = 2;
	
	//************************����������޸ģ���˳�����Ӧ��TPDO���պ�����ҲӦ���ݶ�Ӧ��ϵ����Ӧ���޸� *****************************//
	// void TPDO_DataStoreAndAnalysis(CANopen_Node_t *node ,Whole_SpeedMoto_t *moto, uint8_t *motodata_fb, uint8_t TPDO_num) ���޸� memcpy��˳��//
	Set_TPDO_MapObject[SMOTO_1].TPDO1_MapObject[TPDO_MAP_OBJECT_1] = 0x60770010;   //ʵ��ת�أ����ŷ�
	Set_TPDO_MapObject[SMOTO_1].TPDO1_MapObject[TPDO_MAP_OBJECT_2] = 0x606C0020;	//ʵ��ת��
	
	Set_TPDO_MapObject[SMOTO_1].TPDO2_MapObject[TPDO_MAP_OBJECT_1] = 0x60410010;   //״̬��
	Set_TPDO_MapObject[SMOTO_1].TPDO2_MapObject[TPDO_MAP_OBJECT_2] = 0x60640020;   //实际位置
	
	Set_TPDO_MapObject[SMOTO_2].TPDO1_MapObject[TPDO_MAP_OBJECT_1] = 0x60770010;   //ʵ��ת�أ����ŷ�
	Set_TPDO_MapObject[SMOTO_2].TPDO1_MapObject[TPDO_MAP_OBJECT_2] = 0x606C0020;	//ʵ��ת��
	
	Set_TPDO_MapObject[SMOTO_2].TPDO2_MapObject[TPDO_MAP_OBJECT_1] = 0x60410010;   //״̬��
	Set_TPDO_MapObject[SMOTO_2].TPDO2_MapObject[TPDO_MAP_OBJECT_2] = 0x60640020;   //实际位置
	
	Set_TPDO_MapObject[SMOTO_3].TPDO1_MapObject[TPDO_MAP_OBJECT_1] = 0x60770010;   //ʵ��ת�أ����ŷ�
	Set_TPDO_MapObject[SMOTO_3].TPDO1_MapObject[TPDO_MAP_OBJECT_2] = 0x606C0020;	//ʵ��ת��

	Set_TPDO_MapObject[SMOTO_3].TPDO2_MapObject[TPDO_MAP_OBJECT_1] = 0x60410010;   //״̬��
	Set_TPDO_MapObject[SMOTO_3].TPDO2_MapObject[TPDO_MAP_OBJECT_2] = 0x60640020;   //实际位置
	
	Set_TPDO_MapObject[SMOTO_4].TPDO1_MapObject[TPDO_MAP_OBJECT_1] = 0x60770010;   //ʵ��ת�أ����ŷ�
	Set_TPDO_MapObject[SMOTO_4].TPDO1_MapObject[TPDO_MAP_OBJECT_2] = 0x606C0020;	//ʵ��ת��

	Set_TPDO_MapObject[SMOTO_4].TPDO2_MapObject[TPDO_MAP_OBJECT_1] = 0x60410010;   //״̬��
	Set_TPDO_MapObject[SMOTO_4].TPDO2_MapObject[TPDO_MAP_OBJECT_2] = 0x60640020;   //实际位置

}




void LMS_SpeedMotoRPDOConfig(void)
{
	/*  RPDO whether Set  Begin  */
	SpeedMotoNode[SMOTO_1].nodeRPDO[RPDO1]->RPDO_isuse = USE_THIS_RPDO;
	SpeedMotoNode[SMOTO_1].nodeRPDO[RPDO2]->RPDO_isuse = USE_THIS_RPDO;
	SpeedMotoNode[SMOTO_1].nodeRPDO[RPDO3]->RPDO_isuse = USE_THIS_RPDO;
	
	SpeedMotoNode[SMOTO_2].nodeRPDO[RPDO1]->RPDO_isuse = USE_THIS_RPDO;
	SpeedMotoNode[SMOTO_2].nodeRPDO[RPDO2]->RPDO_isuse = USE_THIS_RPDO;
	SpeedMotoNode[SMOTO_2].nodeRPDO[RPDO3]->RPDO_isuse = USE_THIS_RPDO;
	
	SpeedMotoNode[SMOTO_3].nodeRPDO[RPDO1]->RPDO_isuse = USE_THIS_RPDO;
	SpeedMotoNode[SMOTO_3].nodeRPDO[RPDO2]->RPDO_isuse = USE_THIS_RPDO;
	SpeedMotoNode[SMOTO_3].nodeRPDO[RPDO3]->RPDO_isuse = USE_THIS_RPDO;
	
	SpeedMotoNode[SMOTO_4].nodeRPDO[RPDO1]->RPDO_isuse = USE_THIS_RPDO;
	SpeedMotoNode[SMOTO_4].nodeRPDO[RPDO2]->RPDO_isuse = USE_THIS_RPDO;
	SpeedMotoNode[SMOTO_4].nodeRPDO[RPDO3]->RPDO_isuse = USE_THIS_RPDO;
	/*  RPDO whether Set  End  */
	
	
	/*  RPDO tansType Set  Begin  */
	Set_RPDO_TransType[SMOTO_1][RPDO1] = 0;
	Set_RPDO_TransType[SMOTO_1][RPDO2] = 0;
	Set_RPDO_TransType[SMOTO_1][RPDO3] = 0;
	
	Set_RPDO_TransType[SMOTO_2][RPDO1] = 0;
	Set_RPDO_TransType[SMOTO_2][RPDO2] = 0;
	Set_RPDO_TransType[SMOTO_2][RPDO3] = 0;
	
	Set_RPDO_TransType[SMOTO_3][RPDO1] = 0;
	Set_RPDO_TransType[SMOTO_3][RPDO2] = 0;
	Set_RPDO_TransType[SMOTO_3][RPDO3] = 0;
	
	Set_RPDO_TransType[SMOTO_4][RPDO1] = 0;
	Set_RPDO_TransType[SMOTO_4][RPDO2] = 0;
	Set_RPDO_TransType[SMOTO_4][RPDO3] = 0;
	/*  RPDO tansType Set  End  */
	
	
	/*  RPDO MapNum Set Begin  */
	Set_RPDO_MapNum[SMOTO_1][RPDO1]  = 1;
	Set_RPDO_MapNum[SMOTO_1][RPDO2]  = 2;
	Set_RPDO_MapNum[SMOTO_1][RPDO3]  = 2;
	
	Set_RPDO_MapNum[SMOTO_2][RPDO1]  = 1;
	Set_RPDO_MapNum[SMOTO_2][RPDO2]  = 2;
	Set_RPDO_MapNum[SMOTO_2][RPDO3]  = 2;
	
	Set_RPDO_MapNum[SMOTO_3][RPDO1]  = 1;
	Set_RPDO_MapNum[SMOTO_3][RPDO2]  = 2;
	Set_RPDO_MapNum[SMOTO_3][RPDO3]  = 2;
	
	Set_RPDO_MapNum[SMOTO_4][RPDO1]  = 1;
	Set_RPDO_MapNum[SMOTO_4][RPDO2]  = 2;
	Set_RPDO_MapNum[SMOTO_4][RPDO3]  = 2;
	/*  RPDO MapNum Set  End  */
	
	
	/*******************SMOTO_1 begin******************************/
	Set_RPDO_MapObject[SMOTO_1].RPDO1_MapObject[RPDO_MAP_OBJECT_1] = 0x60FF0020;		//目标速度
//	Set_RPDO_MapObject[SMOTO_1].RPDO1_MapObject[RPDO_MAP_OBJECT_2] = 0x60830010;		//加速度
//	Set_RPDO_MapObject[SMOTO_1].RPDO1_MapObject[RPDO_MAP_OBJECT_3] = 0x60840010;		//减速度
	SpeedMotoNode[SMOTO_1].nodeRPDO[RPDO1]->RPDO_datalen 		   = 4;
	
	Set_RPDO_MapObject[SMOTO_1].RPDO2_MapObject[RPDO_MAP_OBJECT_1] = 0x60830020;		//加速度
	Set_RPDO_MapObject[SMOTO_1].RPDO2_MapObject[RPDO_MAP_OBJECT_2] = 0x60840020;		//加速度
	SpeedMotoNode[SMOTO_1].nodeRPDO[RPDO2]->RPDO_datalen 		   = 8;
	
	Set_RPDO_MapObject[SMOTO_1].RPDO3_MapObject[RPDO_MAP_OBJECT_1] = 0x60A40120;		//加加速度
	Set_RPDO_MapObject[SMOTO_1].RPDO3_MapObject[RPDO_MAP_OBJECT_2] = 0x60A40220;		//减加速度
	SpeedMotoNode[SMOTO_1].nodeRPDO[RPDO3]->RPDO_datalen 		   = 8;
	/*******************SMOTO_1 end******************************/
	
	
	/*******************SMOTO_2 begin******************************/
	Set_RPDO_MapObject[SMOTO_2].RPDO1_MapObject[RPDO_MAP_OBJECT_1] = 0x60FF0020;
//	Set_RPDO_MapObject[SMOTO_2].RPDO1_MapObject[RPDO_MAP_OBJECT_2] = 0x60830010;
//	Set_RPDO_MapObject[SMOTO_2].RPDO1_MapObject[RPDO_MAP_OBJECT_3] = 0x60840010;
	SpeedMotoNode[SMOTO_2].nodeRPDO[RPDO1]->RPDO_datalen 		   = 4;
	
	Set_RPDO_MapObject[SMOTO_2].RPDO2_MapObject[RPDO_MAP_OBJECT_1] = 0x60830020;
	Set_RPDO_MapObject[SMOTO_2].RPDO2_MapObject[RPDO_MAP_OBJECT_2] = 0x60840020;
	SpeedMotoNode[SMOTO_2].nodeRPDO[RPDO2]->RPDO_datalen 		   = 8;
	
	Set_RPDO_MapObject[SMOTO_2].RPDO3_MapObject[RPDO_MAP_OBJECT_1] = 0x60A40120;		//加加速度
	Set_RPDO_MapObject[SMOTO_2].RPDO3_MapObject[RPDO_MAP_OBJECT_2] = 0x60A40220;		//减加速度
	SpeedMotoNode[SMOTO_2].nodeRPDO[RPDO3]->RPDO_datalen 		   = 8;
	/*******************SMOTO_2 end******************************/
	
	
	/*******************SMOTO_3 begin******************************/
	Set_RPDO_MapObject[SMOTO_3].RPDO1_MapObject[RPDO_MAP_OBJECT_1] = 0x60FF0020;
//	Set_RPDO_MapObject[SMOTO_3].RPDO1_MapObject[RPDO_MAP_OBJECT_2] = 0x60830010;
//	Set_RPDO_MapObject[SMOTO_3].RPDO1_MapObject[RPDO_MAP_OBJECT_3] = 0x60840010;
	SpeedMotoNode[SMOTO_3].nodeRPDO[RPDO1]->RPDO_datalen 		   = 4;
	
	
	Set_RPDO_MapObject[SMOTO_3].RPDO2_MapObject[RPDO_MAP_OBJECT_1] = 0x60830020;
	Set_RPDO_MapObject[SMOTO_3].RPDO2_MapObject[RPDO_MAP_OBJECT_2] = 0x60840020;
	SpeedMotoNode[SMOTO_3].nodeRPDO[RPDO2]->RPDO_datalen 		   = 8;
	
	Set_RPDO_MapObject[SMOTO_3].RPDO3_MapObject[RPDO_MAP_OBJECT_1] = 0x60A40120;		//加加速度
	Set_RPDO_MapObject[SMOTO_3].RPDO3_MapObject[RPDO_MAP_OBJECT_2] = 0x60A40220;		//减加速度
	SpeedMotoNode[SMOTO_3].nodeRPDO[RPDO3]->RPDO_datalen 		   = 8;
	/*******************SMOTO_3 end******************************/
	
	
	/*******************SMOTO_4 begin******************************/
	Set_RPDO_MapObject[SMOTO_4].RPDO1_MapObject[RPDO_MAP_OBJECT_1] = 0x60FF0020;
//	Set_RPDO_MapObject[SMOTO_4].RPDO1_MapObject[RPDO_MAP_OBJECT_2] = 0x60830010;
//	Set_RPDO_MapObject[SMOTO_4].RPDO1_MapObject[RPDO_MAP_OBJECT_3] = 0x60840010;
	SpeedMotoNode[SMOTO_4].nodeRPDO[RPDO1]->RPDO_datalen 		   = 4;
	
	Set_RPDO_MapObject[SMOTO_4].RPDO2_MapObject[RPDO_MAP_OBJECT_1] = 0x60830020;
	Set_RPDO_MapObject[SMOTO_4].RPDO2_MapObject[RPDO_MAP_OBJECT_2] = 0x60840020;
	SpeedMotoNode[SMOTO_4].nodeRPDO[RPDO2]->RPDO_datalen 		   = 8;
	
	Set_RPDO_MapObject[SMOTO_4].RPDO3_MapObject[RPDO_MAP_OBJECT_1] = 0x60A40120;		//加加速度
	Set_RPDO_MapObject[SMOTO_4].RPDO3_MapObject[RPDO_MAP_OBJECT_2] = 0x60A40220;		//减加速度
	SpeedMotoNode[SMOTO_4].nodeRPDO[RPDO3]->RPDO_datalen 		   = 8;
	/*******************SMOTO_4 end******************************/
	
	SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->tar_Speed = 0;
	SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->tar_accel = 60000;
	SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->tar_decel = 60000;
	SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->tar_a_accel = 50000;
	SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->tar_d_deccel = 50000;

	SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->tar_Speed = 0;
	SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->tar_accel = 60000;
	SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->tar_decel = 60000;
	SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->tar_a_accel = 50000;
	SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->tar_d_deccel = 50000;
	
	SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->tar_Speed = 0;
	SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->tar_accel = 60000;
	SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->tar_decel = 60000;
	SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->tar_a_accel = 50000;
	SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->tar_d_deccel = 50000;
	
	SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->tar_Speed = 0;
	SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->tar_accel = 60000;
	SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->tar_decel = 60000;
	SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->tar_a_accel = 50000;
	SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->tar_d_deccel = 50000;

}


void LMS_SpeedMotoTPDOConfig(void)
{
	SpeedMotoNode[SMOTO_1].nodeTPDO[TPDO1]->TPDO_isuse = USE_THIS_TPDO;
	SpeedMotoNode[SMOTO_2].nodeTPDO[TPDO1]->TPDO_isuse = USE_THIS_TPDO;
	SpeedMotoNode[SMOTO_3].nodeTPDO[TPDO1]->TPDO_isuse = USE_THIS_TPDO;
	SpeedMotoNode[SMOTO_4].nodeTPDO[TPDO1]->TPDO_isuse = USE_THIS_TPDO;
	
	SpeedMotoNode[SMOTO_1].nodeTPDO[TPDO2]->TPDO_isuse = USE_THIS_TPDO;
	SpeedMotoNode[SMOTO_2].nodeTPDO[TPDO2]->TPDO_isuse = USE_THIS_TPDO;
	SpeedMotoNode[SMOTO_3].nodeTPDO[TPDO2]->TPDO_isuse = USE_THIS_TPDO;
	SpeedMotoNode[SMOTO_4].nodeTPDO[TPDO2]->TPDO_isuse = USE_THIS_TPDO;
	
	Set_TPDO_TransType[SMOTO_1][TPDO1] = 1;
	Set_TPDO_TransType[SMOTO_2][TPDO1] = 1;
	Set_TPDO_TransType[SMOTO_3][TPDO1] = 1;
	Set_TPDO_TransType[SMOTO_4][TPDO1] = 1;
	
	Set_TPDO_TransType[SMOTO_1][TPDO2] = 1;
	Set_TPDO_TransType[SMOTO_2][TPDO2] = 1;
	Set_TPDO_TransType[SMOTO_3][TPDO2] = 1;
	Set_TPDO_TransType[SMOTO_4][TPDO2] = 1;
	
	Set_TPDO_MapNum[SMOTO_1][TPDO1]  = 2;
	Set_TPDO_MapNum[SMOTO_2][TPDO1]  = 2;
	Set_TPDO_MapNum[SMOTO_3][TPDO1]  = 2;
	Set_TPDO_MapNum[SMOTO_4][TPDO1]  = 2;
	
	Set_TPDO_MapNum[SMOTO_1][TPDO2]  = 1;
	Set_TPDO_MapNum[SMOTO_2][TPDO2]  = 1;
	Set_TPDO_MapNum[SMOTO_3][TPDO2]  = 1;
	Set_TPDO_MapNum[SMOTO_4][TPDO2]  = 1;
	
	//************************����������޸ģ���˳�����Ӧ��TPDO���պ�����ҲӦ���ݶ�Ӧ��ϵ����Ӧ���޸� *****************************//
	// void TPDO_DataStoreAndAnalysis(CANopen_Node_t *node ,Whole_SpeedMoto_t *moto, uint8_t *motodata_fb, uint8_t TPDO_num) ���޸� memcpy��˳��//
	Set_TPDO_MapObject[SMOTO_1].TPDO1_MapObject[TPDO_MAP_OBJECT_1] = 0x606C0020;   //实际速度
	Set_TPDO_MapObject[SMOTO_1].TPDO1_MapObject[TPDO_MAP_OBJECT_2] = 0x60780010;	//ʵ实际电流
	Set_TPDO_MapObject[SMOTO_1].TPDO2_MapObject[TPDO_MAP_OBJECT_1] = 0x60410010;   //״状态字
	
	Set_TPDO_MapObject[SMOTO_2].TPDO1_MapObject[TPDO_MAP_OBJECT_1] = 0x606C0020;   //ʵ��ת�أ����ŷ�
	Set_TPDO_MapObject[SMOTO_2].TPDO1_MapObject[TPDO_MAP_OBJECT_2] = 0x60780010;	//ʵ��ת��
	
	Set_TPDO_MapObject[SMOTO_2].TPDO2_MapObject[TPDO_MAP_OBJECT_1] = 0x60410010;   //״̬��
	
	Set_TPDO_MapObject[SMOTO_3].TPDO1_MapObject[TPDO_MAP_OBJECT_1] = 0x606C0020;   //ʵ��ת�أ����ŷ�
	Set_TPDO_MapObject[SMOTO_3].TPDO1_MapObject[TPDO_MAP_OBJECT_2] = 0x60780010;	//ʵ��ת��

	Set_TPDO_MapObject[SMOTO_3].TPDO2_MapObject[TPDO_MAP_OBJECT_1] = 0x60410010;   //״̬��
	
	Set_TPDO_MapObject[SMOTO_4].TPDO1_MapObject[TPDO_MAP_OBJECT_1] = 0x606C0020;   //ʵ��ת�أ����ŷ�
	Set_TPDO_MapObject[SMOTO_4].TPDO1_MapObject[TPDO_MAP_OBJECT_2] = 0x60780010;	//ʵ��ת��

	Set_TPDO_MapObject[SMOTO_4].TPDO2_MapObject[TPDO_MAP_OBJECT_1] = 0x60410010;   //״̬��
}
	


void JMC_PDO_Config(void)
{
	JMC_SpeedMotoRPDOConfig();
	JMC_SpeedMotoTPDOConfig();
}

void LMS_PDO_Config(void)
{
	LMS_SpeedMotoRPDOConfig();
	LMS_SpeedMotoTPDOConfig();
}

/*****************************************
 @function:���ĸ��ٶȵ���ڵ�ʵ���� 
 @param:NULL
author:GTRLTABS
********************************************/
void SpeedMotoNodeInstantiate(void (* MotoType_PDO_Config)(void))
{
	SpeedMotoNodeIDInstantiate();		//����ڵ��ʼ��
	SpeedMotoNodeBootupStateInit();		//�������״̬��ʼ��
	SpeedMotoNodeNMTHeartbeatConfig(); //�������ʱ������
	SpeedMotoSDOInstantiate(); 			//SDOʵ����
	SpeedMotoRPDOInstantiate();			//RPDOʵ����
	SpeedMotoTPDOInstantiate();			//TPDOʵ����
	
	MotoType_PDO_Config();
	
	/*****JMC�������********/
//	JMC_SpeedMotoRPDOConfig();
//	JMC_SpeedMotoTPDOConfig();
	/*****JMC�������********/
	
	/*****LMS�������********/
//	LMS_SpeedMotoRPDOConfig();
//	LMS_SpeedMotoTPDOConfig();
	/*****LMS�������********/
	
	MotoRPDOConfig(&SpeedMotoNode[SMOTO_1], Set_RPDO_TransType[SMOTO_1],Set_RPDO_MapNum[SMOTO_1], &Set_RPDO_MapObject[SMOTO_1]);
	MotoRPDOConfig(&SpeedMotoNode[SMOTO_2], Set_RPDO_TransType[SMOTO_2],Set_RPDO_MapNum[SMOTO_2], &Set_RPDO_MapObject[SMOTO_2]);
	MotoRPDOConfig(&SpeedMotoNode[SMOTO_3], Set_RPDO_TransType[SMOTO_3],Set_RPDO_MapNum[SMOTO_3], &Set_RPDO_MapObject[SMOTO_3]);
	MotoRPDOConfig(&SpeedMotoNode[SMOTO_4], Set_RPDO_TransType[SMOTO_4],Set_RPDO_MapNum[SMOTO_4], &Set_RPDO_MapObject[SMOTO_4]);
	
	MotoTPDOConfig(&SpeedMotoNode[SMOTO_1], Set_TPDO_TransType[SMOTO_1],Set_TPDO_MapNum[SMOTO_1], &Set_TPDO_MapObject[SMOTO_1]);
	MotoTPDOConfig(&SpeedMotoNode[SMOTO_2], Set_TPDO_TransType[SMOTO_2],Set_TPDO_MapNum[SMOTO_2], &Set_TPDO_MapObject[SMOTO_2]);
	MotoTPDOConfig(&SpeedMotoNode[SMOTO_3], Set_TPDO_TransType[SMOTO_3],Set_TPDO_MapNum[SMOTO_3], &Set_TPDO_MapObject[SMOTO_3]);
	MotoTPDOConfig(&SpeedMotoNode[SMOTO_4], Set_TPDO_TransType[SMOTO_4],Set_TPDO_MapNum[SMOTO_4], &Set_TPDO_MapObject[SMOTO_4]);
	
	
}



/*****************************************
 @function:���ٶȵ���ܽṹ��ʵ���� 
 @param:NULL
author:GTRLTABS
********************************************/
void SpeedMotoStructInstantiate(void)
{	
	//���⣺��16������Ĵ�������ָ��δʵ����
	for (uint8_t motonum = 0; motonum<MAX_SPEEDMOTONUM; motonum++)
	{	
		SpeedMotoStruct[motonum].SpeedMotoTarData = &SpeedMotoTarData[motonum];
		
		SpeedMotoStruct[motonum].SpeedMotoFeedBackData = &SpeedMotoFeedBackData[motonum];
	}
	
}


/***************************************************************
 @function:从队列中取分类的速度数据，将其存储到速度电机结构体中
 @param:NULL
 @description:  Task入口函数的实例
author：GTRLTABS
***************************************************************/
void Store_Standard_Speed_Data_To_Struct(void)
{
	//uint16_t recvMq_waittime = 0;
		
	static rt_err_t recvMqresult = 5;
	StandardMotoDataToStoreTask_t  SpeedStandardMotoData;
	rt_err_t  take_mutex_result;
	//RPDO_canframe = &teststruct;
	
	
	take_mutex_result = rt_mutex_take(&Mutex_SpeedTarStructLock,RT_WAITING_FOREVER); //上锁 
	
	if (take_mutex_result == RT_EOK)  //要到锁了再接收数据
	{
		//接收队列消息
		recvMqresult = rt_mq_recv(&Queen_SpeedStandardMotoData,&SpeedStandardMotoData,sizeof(StandardMotoDataToStoreTask_t),RT_WAITING_FOREVER); // 
		
		//成功读到数据就发送，无等待读取
		if(recvMqresult == RT_EOK)
		{
			//NewSpeedData_flag = 1; //表示打包Rpdo数据帧的任务可以发送最新的数据了
			
			switch (SpeedStandardMotoData.FunID)
			{
				case STOP_ONE_MOTO:
				{
					break;
				}
				case STOP_ALL_MOTO:
				{
					break;
				}
				case FAST_STOP:
				{
					break;
				}
				case MOTO_POWER_ON:
				{
					break;
				}
				case SET_TAR_SPEED_MORE:
				{
					switch (SpeedStandardMotoData.DeviceID)
					{
						case SET_MOTO_1TO4:
						{
							SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->tar_Speed = (int32_t)SpeedStandardMotoData.speed[SMOTO_1].mcu_tar_speed;
							SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->tar_Speed = (int32_t)SpeedStandardMotoData.speed[SMOTO_2].mcu_tar_speed;
							SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->tar_Speed = (int32_t)SpeedStandardMotoData.speed[SMOTO_3].mcu_tar_speed;
							SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->tar_Speed = (int32_t)SpeedStandardMotoData.speed[SMOTO_4].mcu_tar_speed;
							break;
						}
						case SET_MOTO_5TO8:
						{
							//目前没用到
							SpeedMotoStruct[SMOTO_5].SpeedMotoTarData->tar_Speed = (int32_t)SpeedStandardMotoData.speed[SMOTO_5].mcu_tar_speed;
							SpeedMotoStruct[SMOTO_6].SpeedMotoTarData->tar_Speed = (int32_t)SpeedStandardMotoData.speed[SMOTO_6].mcu_tar_speed;
							SpeedMotoStruct[SMOTO_7].SpeedMotoTarData->tar_Speed = (int32_t)SpeedStandardMotoData.speed[SMOTO_7].mcu_tar_speed;
							SpeedMotoStruct[SMOTO_8].SpeedMotoTarData->tar_Speed = (int32_t)SpeedStandardMotoData.speed[SMOTO_8].mcu_tar_speed;
							break;
						}
						default :
						{
							break;
						}
					}//switch (SpeedStandardMotoData.DeviceID)
					break;
				}//case SET_TAR_SPEED_MORE:
				case SET_TAR_SPEED_SINGLE:
				{
					switch (SpeedStandardMotoData.DeviceID)
					{
						case SMOTO_1:
						{
							SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->tar_Speed = (int32_t)SpeedStandardMotoData.speed[SMOTO_1].mcu_tar_speed;
							break;
						}
						case SMOTO_2:
						{
							SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->tar_Speed = (int32_t)SpeedStandardMotoData.speed[SMOTO_2].mcu_tar_speed;
							break;
						}
						case SMOTO_3:
						{
							SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->tar_Speed = (int32_t)SpeedStandardMotoData.speed[SMOTO_3].mcu_tar_speed;
							break;
						}
						case SMOTO_4:
						{
							SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->tar_Speed = (int32_t)SpeedStandardMotoData.speed[SMOTO_4].mcu_tar_speed;
							break;
						}
					}//switch (SpeedStandardMotoData.DeviceID)
					break;
				}//case SET_TAR_SPEED_SINGLE:
				case SET_ACC_DEACC_MAXSPEED:
				{
					switch (SpeedStandardMotoData.DeviceID)
					{
						case SMOTO_1:
						{
							SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->max_Speed = (int32_t) SpeedStandardMotoData.speed[SMOTO_1].mcu_max_speed;
							SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->tar_accel = (uint16_t)SpeedStandardMotoData.speed[SMOTO_1].mcu_tar_acc;
							SpeedMotoStruct[SMOTO_1].SpeedMotoTarData->tar_decel = (uint16_t)SpeedStandardMotoData.speed[SMOTO_1].mcu_tar_deacc;
							break;
						}
						case SMOTO_2:
						{
							SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->max_Speed = (int32_t) SpeedStandardMotoData.speed[SMOTO_2].mcu_max_speed;
							SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->tar_accel = (uint16_t)SpeedStandardMotoData.speed[SMOTO_2].mcu_tar_acc;
							SpeedMotoStruct[SMOTO_2].SpeedMotoTarData->tar_decel = (uint16_t)SpeedStandardMotoData.speed[SMOTO_2].mcu_tar_deacc;
							break;
						}
						case SMOTO_3:
						{
							SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->max_Speed = (int32_t) SpeedStandardMotoData.speed[SMOTO_3].mcu_max_speed;
							SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->tar_accel = (uint16_t)SpeedStandardMotoData.speed[SMOTO_3].mcu_tar_acc;
							SpeedMotoStruct[SMOTO_3].SpeedMotoTarData->tar_decel = (uint16_t)SpeedStandardMotoData.speed[SMOTO_3].mcu_tar_deacc;
							break;
						}
						case SMOTO_4:
						{
							SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->max_Speed = (int32_t) SpeedStandardMotoData.speed[SMOTO_4].mcu_max_speed;
							SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->tar_accel = (uint16_t)SpeedStandardMotoData.speed[SMOTO_4].mcu_tar_acc;
							SpeedMotoStruct[SMOTO_4].SpeedMotoTarData->tar_decel = (uint16_t)SpeedStandardMotoData.speed[SMOTO_4].mcu_tar_deacc;
							break;
						}
					}//switch (SpeedStandardMotoData.DeviceID)
					break;
				}//case SET_ACC_DEACC_MAXSPEED:
				default:
				{
					break;
				}
			}//switch (SpeedStandardMotoData.DeviceID)
		}//if(recvMqresult == RT_EOK)
		rt_mutex_release(&Mutex_SpeedTarStructLock);  //解锁
	} //(take_mutex_result == RT_EOK)  //要到锁了再接收数据
	
	
}


/************************************************************
 @function:从队列中取出打包好的速度电机数据，并发送给电机
autohr：GTRLTABS
************************************************************/
RPDO_CANFrame_t testrpdo;
void SendSpeed_CANData_To_Moto(void)
{
	//uint16_t recvMq_waittime = 0;
		
	uint8_t recvMqresult = 5;
	RPDO_CANFrame_t  RPDO_canframe;
	//RPDO_canframe = &teststruct;
	//接收队列消息
	
	if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan2) > 0)  //存在空邮箱
	{
		recvMqresult = rt_mq_recv(&Queen_StandardCANFrame_SpeedDataToMoto,&RPDO_canframe,sizeof(RPDO_CANFrame_t),RT_WAITING_FOREVER);
	
		testrpdo = RPDO_canframe;
		//成功读到数据就发送，无等待读取
		if(recvMqresult == RT_EOK)
		{
			CAN_SendNormalMsg(&hcan2, RPDO_canframe.RPDO_ID, RPDO_canframe.RPDO_Data, RPDO_canframe.RPDO_DATA_DLC);
		}
	}//if (CAN_GetTxMailboxesFreeLevel(&hcan2)== 1)  //存在空邮箱
	else
	{
		rt_thread_mdelay(1);
	}
	
}



/************************************************************
 @function: 根据配置的TPDO映射关系，接收电机反馈的数据
 @param: CANopen_Node_t *node 指示存入哪个节点
 @param: Whole_SpeedMoto_t *moto node对应的电机结构体
 @param:uint8_t TPDO_num 处理哪一个TPDO
 @param:*motodata_fb:CAN的接收结构体
author：GTRLTABS
***********************************************************/
uint16_t  testtorque = 0;
void JMC_Speed_TPDO_DataStoreAndAnalysis(CANopen_Node_t *node ,Whole_SpeedMoto_t *moto, uint8_t *motodata_fb, uint8_t TPDO_num)
{
	
	int32_t 	fb_position;
	int32_t 	fb_speed;
	int16_t 	fb_torque;
	int16_t 	fb_current;
	
	switch (TPDO_num)
	{
		case TPDO1:
		{
			if(node->speed_Or_Position_mode == SPEED_MODE)
			{
				memcpy(node->nodeTPDO[TPDO1]->TPDO_data , motodata_fb , 8);
				
				memcpy(&(fb_torque) ,motodata_fb, 2);   //根据配置的对应关系做修改
				moto->SpeedMotoFeedBackData->fb_torque = fb_torque;
				
				testtorque = moto->SpeedMotoFeedBackData->fb_torque;
				
				memcpy(&(fb_speed) ,&motodata_fb[2], 4); //根据配置的对应关系做修改
				moto->SpeedMotoFeedBackData->fb_speed = fb_speed;
				
			}
			else if (node->speed_Or_Position_mode == POSITION_MODE)
			{
				memcpy(node->nodeTPDO[TPDO1]->TPDO_data , motodata_fb , 8);
			}
			break;
		}//case TPDO1:
		case TPDO2:
		{
			if(node->speed_Or_Position_mode == SPEED_MODE)
			{
				memcpy(node->nodeTPDO[TPDO2]->TPDO_data , motodata_fb , 8);
				
				moto->SpeedMotoFeedBackData->statusWord = (uint16_t)(motodata_fb[1] << 8 | motodata_fb[0]);  //拼出状态字
				
				memcpy(&fb_position , &motodata_fb[2] , 4);
				
				moto->SpeedMotoFeedBackData->fb_position = fb_position;
				
				//teststatewords = moto->SpeedMotoFeedBackData->statusWord;
				switch ( (moto->SpeedMotoFeedBackData->statusWord) & 0x0f)
				{
					case 0x00:
					{
						if ( ((moto->SpeedMotoFeedBackData->statusWord) & 0x40) == 0x00)
						{
							moto->MotoStatus = MOTO_UNINITIALIZED; //设备未初始化
						}
						break;
					}
					case 0x01:
					{
						if ( ((moto->SpeedMotoFeedBackData->statusWord) & 0x60) == 0x20)
						{
							moto->MotoStatus = MOTO_INITIALIZED;
						}
						
						break;
					}
					case 0x03:
					{
						if ( ((moto->SpeedMotoFeedBackData->statusWord) & 0x60) == 0x20)
						{
							moto->MotoStatus = MOTO_POWERON_RUN;
						}
						
						break;
					}
					case 0x07:
					{
						switch ( (moto->SpeedMotoFeedBackData->statusWord) & 0x60)
						{
							case 0x20:
							{
								moto->MotoStatus = MOTO_ENABLE;
								break;
							}
							case 0x00:
							{
								moto->MotoStatus = MOTO_ACTIVE_FASTSTOP;
								break;
							}
							default:
							{
								break;
							}
						}
						break;
					}
					case 0x0f:
					{
						if ( ((moto->SpeedMotoFeedBackData->statusWord) & 0x40) == 0x00)
						{
							moto->MotoStatus = MOTO_ERR_ALARM;
							//SDO_Tx(node, SDO_MESSAGE_W, 0x6040, SUBINDEX_0, BYTE_2, 0x8F);
							//HAL_NVIC_SystemReset();
						}
						break;
					}
					case 0x80:
					{
						if ( ((moto->SpeedMotoFeedBackData->statusWord) & 0x40) == 0x00)
						{
							moto->MotoStatus = MOTO_ERR;
							//SDO_Tx(node, SDO_MESSAGE_W, 0x6040, SUBINDEX_0, BYTE_2, 0x8F);
							//HAL_NVIC_SystemReset();
						}
						break;
					}
					default:
					{
						break;
					}
				
				}//switch (moto->SpeedMotoFeedBackData->statusWord)
				
				
			}//if(node->speed_Or_Position_mode == SPEED_MODE)
			else if (node->speed_Or_Position_mode == POSITION_MODE)
			{
				memcpy(node->nodeTPDO[TPDO2]->TPDO_data , motodata_fb , 8);
			}
			break;
		}//case TPDO2:
		case TPDO3:
		{
			if(node->speed_Or_Position_mode == SPEED_MODE)
			{
				memcpy(node->nodeTPDO[TPDO3]->TPDO_data , motodata_fb , 8);
			}
			else if (node->speed_Or_Position_mode == POSITION_MODE)
			{
				memcpy(node->nodeTPDO[TPDO3]->TPDO_data , motodata_fb , 8);
			}
			break;
		}//case TPDO3:
		case TPDO4:
		{
			if(node->speed_Or_Position_mode == SPEED_MODE)
			{
				memcpy(node->nodeTPDO[TPDO4]->TPDO_data , motodata_fb , 8);
			}
			else if (node->speed_Or_Position_mode == POSITION_MODE)
			{
				memcpy(node->nodeTPDO[TPDO4]->TPDO_data , motodata_fb , 8);
			}
			break;
		}//case TPDO4:
		default :
		{
			break;
		}
	}//switch (TPDO_num)
}
	

/************************************************************
 @function: 根据配置的TPDO映射关系，接收电机反馈的数据
 @param: CANopen_Node_t *node 指示存入哪个节点
 @param: Whole_SpeedMoto_t *moto node对应的电机结构体
 @param:uint8_t TPDO_num 处理哪一个TPDO
 @param:*motodata_fb:CAN的接收结构体
author：GTRLTABS
***********************************************************/
int16_t 	fb_current;
uint8_t 	feedarry[8];
void LMS_Speed_TPDO_DataStoreAndAnalysis(CANopen_Node_t *node ,Whole_SpeedMoto_t *moto, uint8_t *motodata_fb, uint8_t TPDO_num)
{
	uint32_t 	fb_position;
	int32_t 	fb_speed;
	uint16_t 	fb_torque;
	
	
	switch (TPDO_num)
	{
		case TPDO1:
		{
			if(node->speed_Or_Position_mode == SPEED_MODE)
			{
				memcpy(node->nodeTPDO[TPDO1]->TPDO_data , motodata_fb , 8);
				
				memcpy(&(fb_speed) ,motodata_fb, 4);   //根据配置的对应关系做修改
				moto->SpeedMotoFeedBackData->fb_speed = fb_speed;
				
				memcpy(&(fb_current) ,&motodata_fb[4], 2); //根据配置的对应关系做修改
				moto->SpeedMotoFeedBackData->fb_current = (int16_t)fb_current;
				
				for(uint8_t i = 0;i<8;i++)
				{
					feedarry[i]=motodata_fb[i];
				}
			}
			else if (node->speed_Or_Position_mode == POSITION_MODE)
			{
				memcpy(node->nodeTPDO[TPDO1]->TPDO_data , motodata_fb , 8);
			}
			break;
		}//case TPDO1:
		case TPDO2:
		{
			if(node->speed_Or_Position_mode == SPEED_MODE)
			{
				memcpy(node->nodeTPDO[TPDO2]->TPDO_data , motodata_fb , 8);
				
				moto->SpeedMotoFeedBackData->statusWord = (uint16_t)(motodata_fb[1] << 8 | motodata_fb[0]);  //拼出状态字
				
				switch ( (moto->SpeedMotoFeedBackData->statusWord) & 0x0f)
				{
					case 0x00:
					{
						if ( ((moto->SpeedMotoFeedBackData->statusWord) & 0x40) == 0x00)
						{
							moto->MotoStatus = MOTO_UNINITIALIZED; //设备未初始化
						}
						break;
					}
					case 0x01:
					{
						if ( ((moto->SpeedMotoFeedBackData->statusWord) & 0x60) == 0x20)
						{
							moto->MotoStatus = MOTO_INITIALIZED;
						}
						
						break;
					}
					case 0x03:
					{
						if ( ((moto->SpeedMotoFeedBackData->statusWord) & 0x60) == 0x20)
						{
							moto->MotoStatus = MOTO_POWERON_RUN;
						}
						
						break;
					}
					case 0x07:
					{
						switch ( (moto->SpeedMotoFeedBackData->statusWord) & 0x60)
						{
							case 0x20:
							{
								moto->MotoStatus = MOTO_ENABLE;
								break;
							}
							case 0x00:
							{
								moto->MotoStatus = MOTO_ACTIVE_FASTSTOP;
								break;
							}
							default:
							{
								break;
							}
						}
						break;
					}
					case 0x0f:
					{
						if ( ((moto->SpeedMotoFeedBackData->statusWord) & 0x40) == 0x00)
						{
							moto->MotoStatus = MOTO_ERR_ALARM;
						}
						break;
					}
					case 0x80:
					{
						if ( ((moto->SpeedMotoFeedBackData->statusWord) & 0x40) == 0x00)
						{
							moto->MotoStatus = MOTO_ERR;
						}
						break;
					}
					default:
					{
						break;
					}
				
				}//switch (moto->SpeedMotoFeedBackData->statusWord)
				
				
			}//if(node->speed_Or_Position_mode == SPEED_MODE)
			else if (node->speed_Or_Position_mode == POSITION_MODE)
			{
				memcpy(node->nodeTPDO[TPDO2]->TPDO_data , motodata_fb , 8);
			}
			break;
		}//case TPDO2:
		case TPDO3:
		{
			if(node->speed_Or_Position_mode == SPEED_MODE)
			{
				memcpy(node->nodeTPDO[TPDO3]->TPDO_data , motodata_fb , 8);
			}
			else if (node->speed_Or_Position_mode == POSITION_MODE)
			{
				memcpy(node->nodeTPDO[TPDO3]->TPDO_data , motodata_fb , 8);
			}
			break;
		}//case TPDO3:
		case TPDO4:
		{
			if(node->speed_Or_Position_mode == SPEED_MODE)
			{
				memcpy(node->nodeTPDO[TPDO4]->TPDO_data , motodata_fb , 8);
			}
			else if (node->speed_Or_Position_mode == POSITION_MODE)
			{
				memcpy(node->nodeTPDO[TPDO4]->TPDO_data , motodata_fb , 8);
			}
			break;
		}//case TPDO4:
		default :
		{
			break;
		}
	}//switch (TPDO_num)
}


